﻿using System;
using System.Collections.Generic;
using System.Text;

namespace LocalizationTest.Tools
{
  public abstract class Geometry
  {
    public static double AngleTo(Pose2D from, Vector2 to)
    {
      Pose2D relPos = new Pose2D(to) - from;
      return Math.Atan2(relPos.translation.y, relPos.translation.x);
    }

    public static double DistanceTo(Pose2D from, Vector2 to)
    {
      return (new Pose2D(to) - from).translation.Abs();
    }

  }
}
